//	Thor PIC code
// 	autonomous probabilistic ball-fetching robot
//	daniel ford 2011

// need to update to only update state when data received from USART
// should also have an init command that resets all state variables
// otherwise just publish state (upon request?)
// also need to change to interrupt-driven code
 
#include <p18f4580.h> 
#include <delays.h>   
#include <usart.h>    
#include <timers.h>
#include <stdlib.h> 
#include <string.h>
#include <stdio.h> 

#include "PIC_init.h"
#include "PIC_constants.h"

// serial storage variables
char driveMtr[5]  = "+000";
char steerMtr[2]  = "5";      // 0 turns left, 9 turns right, anything else stays center
                              // will need to actually turn the motor back to center... somehow
char gripSense[4] = "000";
char gripPos[4]   = "000";
char frontBump[2] = "0";
char rearBump[2]  = "0";
char camTilt[4]   = "000";
char redState[2]  = "0";
char yelState[2]  = "0";
char grnState[2]  = "0";
         
// ISR prototypes         
void ext_isr (void);        
void timer_isr (void);

//prototypes for ASM ISR
void drive_PWM(	unsigned int ASMcount1, unsigned int count1_dc,
                unsigned int count1_max, unsigned int driveDir);
void steer_PWM(	unsigned int ASMcount2, unsigned int count2_dc,
                unsigned int count2_max, unsigned int steerDir);

// motor control PWM variables
unsigned int ASMcount1 = 0;
unsigned int count1_dc = 0;
unsigned int count1_max = 128;
unsigned int driveDir = STOP;

unsigned int ASMcount2 = 0;
unsigned int count2_dc = 0;
unsigned int count2_max = 128;
unsigned int steerDir = STOP;

// gripper variables
unsigned int gripPos_i = HAND_OPEN;
unsigned int servoPulse = 0;

// serial comms prototypes
void getNewState(void);
void sendStatePacket(void);
void send(char* msg, int size);

// data conversion prototypes
int ctoi(char in);
char itoc(int in);

/********** INTERRUPTS ************/

#pragma code _HIGH_INTERRUPT_VECTOR=0x0000008
 
void high_ISR (void)
{   
 
// button interrupt disabled
 
_asm     
    goto ext_isr   
 _endasm
}
 
#pragma code _LOW_INTERRUPT_VECTOR=0x0000018
#pragma interruptlow timer_isr
void timer_isr (void)
{

  // send PWM commands to drive motors
  drive_PWM(ASMcount1,count1_dc,count1_max, driveDir);
  //steer_PWM(ASMcount2,count2_dc,count2_max, driveDir);

  // control gripper servo
  if( servoPulse <= gripPos_i )
    GRIPPER = 0x01;
  if( (servoPulse > gripPos_i) && (servoPulse < (SRVODLY+gripPos_i)) )
    GRIPPER = 0x00;
  if( servoPulse >= (SRVODLY+gripPos_i) )
    servoPulse = 0;
  
  ++servoPulse;
  
  // reset timer to interrupt in 0.1 ms 
  WriteTimer0(timer_offset);
 
  // reset interrupt bit 
  INTCONbits.TMR0IF=0;
 
}
 
#pragma interrupt ext_isr
void ext_isr (void)
{

// button interrupt disabled to permit polling
 
}

// main function 
void main(void)
{
 
  char cmd=0;

	// initialize ports, timers, etc.
	init_PIC();

	//main loop
  while(1)
	{ 
  
		while(!DataRdyUSART());   // wait for data to appear on serial port
    cmd = getcUSART();        // log command
    if(cmd == 'W')
      getNewState();          // get new state data 
                              // need to add a function to actually update the state
    else if(cmd == 'R')
      sendStatePacket();      // if not, send current state state 

  }
  CloseTimer0();
  CloseUSART();
  
}

// update state based on serial input
void getNewState()
{

  int driveTemp = 0, steer = 0;

  // get serial input and assign to storage variables       
  getsUSART(driveMtr, 5);
  getsUSART(steerMtr, 2);
  getsUSART(gripSense, 4);
  getsUSART(gripPos, 4);
  getsUSART(frontBump, 2);
  getsUSART(rearBump, 2);
  getsUSART(camTilt, 4);
	getsUSART(redState, 2);
	getsUSART(yelState, 2);
	getsUSART(grnState, 2);  
  
  // convert string variables to ints for control

  steer = atoi(steerMtr);          // steering control
  if(steer == 0)
    { steerDir = LEFT;
      count2_dc = STEER; }
  else if(steer == 9)
    { steerDir = RIGHT;
      count2_dc = STEER; }
  else
    { driveDir & 1<<4 & 1<<5;
      count2_dc = 0; }
  
  driveTemp = atoi(driveMtr);      // drive motor control  
  if(driveTemp > 0)
    { driveDir = FORWARD | steerDir;
      count1_dc = driveTemp; }
  if(driveTemp < 0 )
    { driveDir = REVERSE | steerDir;
      count1_dc = driveTemp; }
  if(driveTemp == 0)
    { driveDir = STOP;
      count1_dc = 0; }
  
  // servo control
  gripPos_i = atoi(gripPos);
  
  ledRed = atoi(redState);        // LED control
  ledYel = atoi(yelState);
  ledGrn = atoi(grnState);
  
}

// send current state of chassis
void sendStatePacket()
{
 
  putcUSART('F');
  while(BusyUSART());
  
  putsUSART(driveMtr);
  putsUSART(steerMtr);
  putsUSART(gripSense);
  putsUSART(gripPos);
  putsUSART(frontBump);
  putsUSART(rearBump);
  putsUSART(camTilt);  
	putsUSART(redState);
	putsUSART(yelState);
	putsUSART(grnState);
	
}

// send string w/o null character over USART
void send(char* msg, int size)
{
  int i = 0;
  for(i=0;i<size;++i)
  {
    putcUSART(msg[i]);
    while(BusyUSART());
  }
}

// convert string between 0 and 9 to int
int ctoi(char in)
{
  int out = in - '0';
  return out;
}

// convert int between 0 and 9 to char
char itoc(int in)
{
  char out = in + '0';
  return out;
}
